#include "pidControl.h"
#include "main.h"
#include "tim.h"
/*拟合函数参数宏定义*/
/*
#define p00 560.2
#define p10 34.45
#define p01 0.376
#define p20 0.194
#define p11 (-0.064)
#define p02 0.002
*/
#define p00 432.5
#define p10 77.85
#define p01 -0.662
#define p20 -1.46
#define p11 0.064
#define p30 0.016
#define p21 -0.002

/*init private begin*/
int button=0;
tPID Fan_PID;
/*init private end*/

/**************************************************************************
函数功能：读取旋钮的值
入口参数：无
返回  值：无
**************************************************************************/
void Read_Encoder(int *button)
{
    *button+= -(short)TIM3 -> CNT; TIM3 -> CNT=0;
}

/**************************************************************************
函数功能：风机PWM
入口参数：PWM值，0-5000
返回  值：值
**************************************************************************/
void Load(int moto1,int moto2)
{
    if(moto2>0){
        HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_SET);
        HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_RESET);//正转
        __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,moto2);

    }
}

/**************************************************************************
函数功能：pid初始化
入口参数：无
返回  值：值
**************************************************************************/
void PID_Init(){
    Fan_PID.Kp=0;
    Fan_PID.Kd=50;
}

/**************************************************************************
函数功能：pid计算
入口参数：无
返回  值：值
**************************************************************************/
float PID_Fan_Calc(tPID *PIDX,float Angle_Bias){
    static float Last_Bias=0;
    PIDX->output=PIDX->Kp*Angle_Bias+PIDX->Kd*(Angle_Bias-Last_Bias); //计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数

    if(PIDX->output > 2000 )   PIDX->output = 2000;
    Last_Bias = Angle_Bias;
    return PIDX->output;
}

/**************************************************************************
函数功能：风扇控制帆板
入口参数：无
返回  值：值
**************************************************************************/
void Fan_Control(int pwm) {
    // 设置定时器的比较寄存器
    if(pwm>=4990)
        pwm=4990;
    else if(pwm<=0)
        pwm=0;
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pwm);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, pwm);
}

/**************************************************************************
函数功能：旋钮控制舵机
入口参数：无
返回  值：值
**************************************************************************/
void Position_Control(int *DuoJi_Position) {
    if(*DuoJi_Position>=84) *DuoJi_Position=84;
    if(*DuoJi_Position<=-84) *DuoJi_Position=-84;
    __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,150+(*DuoJi_Position)/135.0*100);
    __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_2,150-(*DuoJi_Position)/90.0*100);
}

/**************************************************************************
函数功能：声光提醒
入口参数：flag
返回  值：值
**************************************************************************/
void LightBeef(uint8_t flag) {
    if(flag==0) {
        HAL_GPIO_WritePin(LED_GPIO_Port,LED_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(Beef_GPIO_Port,Beef_Pin,GPIO_PIN_SET);
    }else if(flag==1) {
        HAL_GPIO_WritePin(LED_GPIO_Port,LED_Pin,GPIO_PIN_SET);
        HAL_GPIO_WritePin(Beef_GPIO_Port,Beef_Pin,GPIO_PIN_RESET);
    }

}
/**************************************************************************
函数功能：角度和电机PWM的拟合函数
入口参数：角度值和舵机值
返回  值：计算的PWM的结果
**************************************************************************/
int NiHe_Model_Final(int Sail_Angle,int DuoJi_Angle) {
    //return p00 + p10*Duoji_angle + p01*Sail_angle + p20*Duoji_angle*Duoji_angle + p11*Duoji_angle*Sail_angle + p02*Sail_angle*Sail_angle;
    return p00 + p10*Sail_Angle + p01*DuoJi_Angle + p20*Sail_Angle*Sail_Angle + p11*Sail_Angle*DuoJi_Angle + p30*Sail_Angle*Sail_Angle*Sail_Angle + p21*Sail_Angle*Sail_Angle*DuoJi_Angle;
}

/**************************************************************************
函数功能：限幅
入口参数：被限幅的变量，限幅的值
返回  值：值
**************************************************************************/
void Limit(int *BeLimited,int Range) {
    if(*BeLimited>=Range) *BeLimited=Range;
    else if(*BeLimited<=-Range) *BeLimited=-Range;

}
/**************************************************************************
函数功能：开机音乐
入口参数：flag
返回  值：值
**************************************************************************/
void PlaySimpleMusic() {
    // 音乐的持续时间 3秒
    int duration = 3000;

    // 第一段：蜂鸣器响 200ms
    LightBeef(1);  // 开启蜂鸣器
    HAL_Delay(200); // 延时 200ms

    // 第二段：蜂鸣器关闭 100ms
    LightBeef(0);  // 关闭蜂鸣器
    HAL_Delay(100); // 延时 100ms

    // 第三段：蜂鸣器响 300ms
    LightBeef(1);  // 开启蜂鸣器
    HAL_Delay(300); // 延时 300ms

    // 第四段：蜂鸣器关闭 100ms
    LightBeef(0);  // 关闭蜂鸣器
    HAL_Delay(100); // 延时 100ms

    // 第五段：蜂鸣器响 400ms
    LightBeef(1);  // 开启蜂鸣器
    HAL_Delay(400); // 延时 400ms

    // 第六段：蜂鸣器关闭 200ms
    LightBeef(0);  // 关闭蜂鸣器
    HAL_Delay(200); // 延时 200ms

    // 第七段：蜂鸣器响 300ms
    LightBeef(1);  // 开启蜂鸣器
    HAL_Delay(300); // 延时 300ms

    // 第八段：蜂鸣器关闭 200ms
    LightBeef(0);  // 关闭蜂鸣器
    HAL_Delay(200); // 延时 200ms
}